
/**
 * @author Thierry Bertin-Mahieux and Jin Yoshikawa
 * @data 04/04/2009
 * Screen where we can see possible paths
 */

import javax.swing.*;

import java.awt.event.*;
import java.awt.geom.*;
import java.awt.Color;
import java.util.*;
import java.awt.*;
import java.io.*;


// global parameters
class Global {
    public static int SAFETY_MARGIN = 300;		// safety margin along obstacles
    public static int SAFETY_MARGIN2 = 3;               // extra safety margin for actual points
    public static double SCALING = 1;			// scaling for visualization
    public static double X_TRANS = 250;			// translations for visualization
    public static double Y_TRANS = 200;
    public static int VISU_HEIGHT = 300;		// visualization height and weight
    public static int VISU_WIDTH = 600;
    public static int INTERSECT_PRECISION = 50;	// intersect precision
    public static double ANGLE_PRECISION = Math.PI / 360.;	// when looking for closest crash obstacle
    public static int POINT_RADIUS = 200;		// radius of a simple point
    public static int ROBOT_RADIUS = 25;
}

// plan maker, main class
public class PlanMaker {
	
    // keep a list of point, dimensions and the center
    //LinkedList<Point2D> points = new LinkedList<Point2D>();
    private LinkedList<Obstacle> obstacles;
    private LinkedList<Point2D> interestingpoints;
    private Point2D start;
    private Point2D goal;
    private int num_obstacles = 0;
    private double[][] dist_table;
    private OpenComPort iRobotBAM;
    
    // student actions
    StudentActions sactions = null;
    
    // default constructor
    public PlanMaker() {
    	obstacles = null;
    	interestingpoints = null;
    	start = goal = null;
    	num_obstacles = 0;
    	dist_table = null;
    	iRobotBAM = null;
    }

    public void read_startgoal_file(String filename){
    	Point2D tempstart = start;
    	Point2D tempgoal = goal;
    	File file = new File(filename);
    	Scanner scanner = null;
    	try {
    		scanner = new Scanner(file);
    	}
    	catch (FileNotFoundException e) {
    		System.out.println("File not found: " + filename);
    	}
    	try {
    		// first value, num obstacles
    		double startx = scanner.nextDouble();
    		double starty = scanner.nextDouble();
    		start = new Point2D.Double(startx*1000, starty*1000);
    		double goalx = scanner.nextDouble();
    		double goaly = scanner.nextDouble();
    		goal = new Point2D.Double(goalx*1000, goaly*1000);
    		if (scanner.hasNextDouble())
    			throw new InputMismatchException();
    	}
    	catch (InputMismatchException e) {
    		System.out.println("Mismatch exception: " + e);
    		start = tempstart;
    		goal = tempgoal;
    	}
    	scanner.close();
    	if (obstacles != null && start != null && goal != null){
        	populate_interestingpoints();
        	compute_distances();
    	}
    }
    
    // read description file of the course
    public void read_obstacle_file(String filename){
    	LinkedList<Obstacle> tempobstacles = obstacles;
    	int tempnumobstacles = num_obstacles;
    	File file = new File(filename);
    	Scanner scanner = null;
    	try {
    		scanner = new Scanner(file);
    	}
    	catch (FileNotFoundException e) {
    		System.out.println("File not found: " + filename);
    	}
    	try {
    		// first value, num obstacles
    		num_obstacles = scanner.nextInt();
    		// iter on obstacles
    		obstacles = new LinkedList<Obstacle>();
    		for (int obst = 0; obst < num_obstacles; obst++){
    			int num_vert = scanner.nextInt();
    			Obstacle ob = new Obstacle(num_vert);
    			if (obst == 0) {
    				ob.setWall(true);
    				System.out.println("we set wall");
    			}
    			// iter on vertices
    			for (int v = 0; v < num_vert; v++){
    				double cX = scanner.nextDouble();
    				double cY = scanner.nextDouble();
    				// conversion, m -> mm
    				cX *= 1000;
    				cY *= 1000;
    				// add to obstacle
    				ob.addVertice(cX,cY);
    			}
    			ob.lock();
    			obstacles.add(ob);
    		}
    	}
    	catch (InputMismatchException e) {
    		System.out.println("Mismatch exception: " + e);
    		obstacles = tempobstacles;
    		num_obstacles = tempnumobstacles;
    	}
    	scanner.close();
    	assert( num_obstacles == obstacles.size() );
    	if (obstacles != null && start != null && goal != null){
        	populate_interestingpoints();
        	compute_distances();
    	}
    }

    private void populate_interestingpoints(){
    	LinkedList<Point2D> res = new LinkedList<Point2D>();
    	res.add(start);
    	res.add(goal);
    	// get points from polygons
    	for (int o = 0; o < num_obstacles/*obstacles.size()*/; o++) {
    	    LinkedList<Point2D> opoints = obstacles.get(o).possiblePathPoints();
    	    for (Point2D p : opoints) {
    	    	res.add(p);
    	    }
    	}
    	// get points from polygons intersections
    	Polygon p1, p2;
    	for (int o1 = 0; o1 < num_obstacles/*obstacles.size()-1*/; o1++)
    		for (int o2 = o1+1; o2 < num_obstacles/*obstacles.size()*/; o2++){
    			p1 = obstacles.get(o1).getPolyExt2();
    			p2 = obstacles.get(o2).getPolyExt2();
    			LinkedList<Point2D> opoints = PolyOps.intersections_polys(p1,p2);
    			for (Point2D p : opoints)
    				res.add(p);
    		}
    	// remove duplicates
    	interestingpoints = new LinkedList<Point2D>();
    	boolean has_dupe;
    	for (int k1 = 0; k1 < res.size() - 1; k1++){
    		has_dupe = false;
    		for (int k2 = k1 + 1; k2 < res.size(); k2++){
    			if (res.get(k1).equals( res.get(k2) )){
    				has_dupe = true;
    				//System.out.println("dupe");
    				break;
			    }
		    }
    		if(!has_dupe)
    			interestingpoints.add( res.get(k1) );
	    }
    	// don't forget last point!!!
    	interestingpoints.add( res.get( res.size() - 1 ) );
    	// done..
    }

    // get the min/max of X and Y of all obstacles
    public double get_minX_obstacles() {
    	double res = 1e14;
    	if (obstacles!=null)
    		for (int o = 0; o < num_obstacles/*obstacles.size()*/; o++) {
    			double x = obstacles.get(o).getPoly().getBounds().getX();
    			if ( x < res )
    				res = x;
    		}
    	return res;
    }
    
    public double get_maxX_obstacles() {
    	double res = -1e14;
    	if (obstacles!=null)
    		for (int o = 0; o < num_obstacles/*obstacles.size()*/; o++) {
    			double x = obstacles.get(o).getPoly().getBounds().getX();
    			x += obstacles.get(o).getPoly().getBounds().getWidth();
    			if ( x > res )
    				res = x;
    		}
    	return res;
    }
    
    public double get_minY_obstacles() {
    	double res = 1e14;
    	if (obstacles!=null)
    	for (int o = 0; o < num_obstacles/*obstacles.size()*/; o++) {
    		double y = obstacles.get(o).getPoly().getBounds().getY();
    		if ( y < res )
    			res = y;
    	}
    	return res;
    }
    
    public double get_maxY_obstacles() {
    	double res = -1e14;
    	if (obstacles!=null)
    	for (int o = 0; o < num_obstacles/*obstacles.size()*/; o++) {
    		double y = obstacles.get(o).getPoly().getBounds().getY();
    		y += obstacles.get(o).getPoly().getBounds().getWidth();
    		if ( y > res )
    			res = y;
    	}
    	return res;
    }
    
    // ************** GETTERS ********************************
    
    public LinkedList<Obstacle> getObstacles(){
    	return obstacles;
    }
    
    public int getNumObstacles(){
    	return num_obstacles;
    }
    
    public LinkedList<Point2D> getInterestingPoints(){
    	return interestingpoints;
    }
    
    public Point2D getStart(){
    	return start;
    }
    
    public Point2D getGoal(){
    	return goal;
    }
    
    public double[][]getdisttable(){
    	return dist_table;
    }
   

    // *************** PATH FUNCTIONS ************************
    
    // returns true if a segments intersects with any obstacle
    private boolean seg_crashes(double x1, double y1, double x2, double y2){
    	Point2D p1 = new Point2D.Double(x1,y1);
    	Point2D p2 = new Point2D.Double(x2,y2);
    	Obstacle res = seg_crashes_return_obst(p1,p2);
    	if (res == null)
    		return false;
    	return true;
    }
    private Obstacle seg_crashes_return_obst(Point2D p1, Point2D p2) {
    	for (int o = 0; o < num_obstacles/*obstacles.size()*/; o++)
    		if ( obstacles.get(o).intersect_ext(p1.getX(),p1.getY(),p2.getX(),p2.getY()) )
		    {
    			return obstacles.get(o);
		    }
		return null;
    }

    // compute points distances
    private void compute_distances(){
    	if (interestingpoints == null){
    		dist_table = null;
    		return;
    	}
    	// initialize table with zeros
    	int n = interestingpoints.size();
    	dist_table = new double[n][n];
    	for (int k1 = 0; k1 < n; k1++)
    		for (int k2 = 0; k2 < n; k2++)
    			dist_table[k1][k2] = 0;
    	// get distances (assumes symmetry)
    	Point2D p1, p2;
    	for (int k1 = 0; k1 < n-1; k1++)
    		for (int k2 = k1; k2 < n; k2++){
    			p1 = interestingpoints.get(k1);
    			p2 = interestingpoints.get(k2);
    			if (seg_crashes(p1.getX(),p1.getY(),p2.getX(),p2.getY()))
			    {
    				dist_table[k1][k2] = dist_table[k2][k1] = 0;
			    }
    			else
    				dist_table[k1][k2] = dist_table[k2][k1] = distance_2points(p1,p2);
    		}
    }
    
    // checks if a given point is a point of the wall
   /* private boolean point_is_on_wall(Point2D p){
    	System.out.println("point_is_on_wall, not implemented yet");
    	return true;
    }*/

    // Dijkstra
    public LinkedList<Point2D> dijkstraPath(){
    	if (interestingpoints==null || start==null || goal==null)
    		return null;
    	int n = interestingpoints.size();
    	double min_dist_table[][] = new double[n][n];
    	LinkedList<Integer> path = new LinkedList<Integer>();
    	for (int i = 0; i < n; i++)
    		for (int j = 0; j < n; j++){
    			min_dist_table[i][j] = Double.POSITIVE_INFINITY;
    		}
    	min_dist_table[0][0] = 0;
    	while (!goalreached(min_dist_table, n)){
    		int closest[] = getClosestVertex(min_dist_table, n);
    		if (closest[0] == 0 && closest[1] == 0){
    			System.out.println("No path");
    			break;
    		}
    		min_dist_table[closest[0]][closest[1]] = distance_start2vertex(closest[0],min_dist_table,n)+dist_table[closest[0]][closest[1]];
    	}
    	int current = 1;
    	path.addFirst(new Integer(current));
    	while (current != 0){
    		int parent = parentof(current,min_dist_table,n);
    		if (parent == Integer.MAX_VALUE){
    			System.out.println("No path");
    			break;
    		}
    		path.addFirst(new Integer(parent));
    		current = parent;
    	}
    	LinkedList<Point2D> path2 = new LinkedList<Point2D>();
    	for (int i = 0; i<path.size(); i++){
    		path2.add(interestingpoints.get(path.get(i)));
    	}
    	System.out.println("Path="+path);
    	return path2;
    }
    
    // actually launches robot!!!
    public LinkedList<Point2D> executePath(LinkedList<Point2D> path, boolean testmode){
    	if (testmode)
    		iRobotBAM = null;
    	else
    		iRobotBAM = new OpenComPort();
    	LinkedList<Point2D> executedpath = new LinkedList<Point2D>();
    	if (!testmode){
    		if(iRobotBAM.SetupBAM()==1){
    			System.err.println("Error opening connection");
    			return null;
    		}
    		else{
    			System.out.println("Connected!");
    		}
    		byte[] data = new byte[2];
    		data[0] = (byte)128;  //Direct Drive command
    		data[1] = (byte)132;  //Full Mode
    		iRobotBAM.Write(data);
    	}
    	sactions = new StudentActions(iRobotBAM);
    	int n = path.size();
    	Point2D p1,p2;
    	p1 = path.get(0);
    	executedpath.add(p1);
    	double presentangle = 180/Math.PI*Math.atan2(goal.getY()-start.getY(),goal.getX()-start.getX());
    	for (int i = 1; i < n; i++){
    		p2 = path.get(i);
    		int dist = (int)distance_2points(p1,p2);
    		System.out.println("going from:"+p1+" to "+p2);
    		System.out.println("ref:("+start+","+goal+")");
    		int angle = sactions.TurnToPoint(p2, p1, start, goal, true);
    		System.out.println("angle="+angle);
    		System.out.println("dist="+dist+"\n");
    		sactions.GoDistance(dist);
    		presentangle-=angle;
    		executedpath.add(new Point2D.Double(p1.getX()+dist*Math.cos(Math.PI/180*presentangle),p1.getY()+dist*Math.sin(Math.PI/180*presentangle)));
    		// check if we bump
    		//double bumps[] = sactions.GetWallBumps();
    		//if (bumps[0] == 1 || bumps[1] == 1)
    		//	return executedpath;
    		// no bump
    		p1 = p2;
    	}
    	return executedpath;
    }
    
    private int parentof(int i, double min_dist_table[][], int n){
    	int parent = Integer.MAX_VALUE;
    	for (int k = 0; k < n; k++){
    		if (min_dist_table[k][i] < Double.POSITIVE_INFINITY)
    			parent = k;
    	}
    	return parent;
    }
    
    private boolean goalreached(double min_dist_table[][], int n){
    	boolean reached = false;
    	int i = 0;
    	while (!reached && i < n){
    		if (min_dist_table[i][1] < Double.POSITIVE_INFINITY)
    			reached = true;
    		i++;
    	}
    	return reached;
    }
    
    /**
     * Returns the index of the closest vertex to a graph of visited vertices
     * in interestingpoints
     * @param vertexIndex vertex to be compared to.
     * @param vertices List of vertices to choose from.
     * @return
     */
    private int []getClosestVertex(double min_dist_table[][], int n){
    	int closest[] = {0,0};
    	double shortestdist = Double.POSITIVE_INFINITY;
    	for (int i = 0; i < n; i++)
    		for (int j = 0; j < n; j++){
    			boolean visited = false;
    			for (int k = 0; k < n; k++)
    				if (min_dist_table[k][j] < Double.POSITIVE_INFINITY)
    					visited = true;
    			if (!visited && dist_table[i][j] > 0 && (distance_start2vertex(i, min_dist_table, n)+dist_table[i][j]) < shortestdist){
    				shortestdist = distance_start2vertex(i, min_dist_table, n)+dist_table[i][j];
    				closest[0] = i;
    				closest[1] = j;
    			}
    		}
    	return closest;
    }
    
    private double distance_start2vertex(int vertex, double min_dist_table[][], int n){
    	double mindist = Double.POSITIVE_INFINITY;
    	for (int i = 0; i < n; i++){
    		if (min_dist_table[i][vertex] < mindist)
    			mindist = min_dist_table[i][vertex];
    	}
    	return mindist;
    }
    
    /**
     * Prints the distance matrix to the screen
     * @param size Size of the NxN distance matrix
     */
    public void printDistanceMatrix(){
    	int size = interestingpoints.size();
    	for (int i = 0; i < size; i++){
    		for (int j = 0; j < size; j++){
    			System.out.print((int)dist_table[i][j]+"\t");
    		}
    		System.out.println();
    	}
    }

    // computes distance between two Point2D
    private double distance_2points(Point2D p1, Point2D p2){
    	double width = p1.getX() - p2.getX();
    	double height = p1.getY() - p2.getY();
    	return Math.sqrt( width*width + height*height );
    }


    // *************** END PATH FUNCTIONS ********************

    // *************** CRASH FUNCTIONS ***********************
    
    // try increasing error angles until crashes into an obstacle
    // returns a point, or null if no obstacle found
    public Point2D find_probable_crash_obstacle(Point2D src, Point2D dest) {
    	double angle = Global.ANGLE_PRECISION;
    	Obstacle obst = null;
    	Point2D p = new Point2D.Double(0,0);
    	while(angle < Math.PI / 2) {
    		// pos angle
    		p = PolyOps.seg_plus_angle(src,dest, angle);
    		obst = seg_crashes_return_obst(src,p);
    		if (obst != null)
    			break;
    		// neg angle
    		p = PolyOps.seg_plus_angle(src,dest, -angle);
    		obst = seg_crashes_return_obst(src,p);
    		if (obst != null)
    			break;
    		// increment angle
    		angle += Global.ANGLE_PRECISION;
    	}
    	// if no obstacle found
    	if (obst == null)
    		return null;
    	// where are we?
    	Point2D contact = obst.touch_where(src.getX(),src.getY(),p.getX(),p.getY());
    	// remove robot radius
    	double seg_length = this.distance_2points(src,contact);
    	double scaling = Global.ROBOT_RADIUS / seg_length;
    	double robot_x = contact.getX() - (contact.getX() - src.getX()) * scaling;
    	double robot_y = contact.getY() - (contact.getY() - src.getY()) * scaling;
    	Point2D robot_pos = new Point2D.Double(  robot_x , robot_y );
    	// return
    	return robot_pos;
    }
    
    // *************** END CRASH FUNCTIONS *******************
    
    
    
    public String toString() {
    	String s = "PlanMaker, [";
    	for (int k = 0; k < num_obstacles/*obstacles.size()*/; k++){
    		s += obstacles.get(k).toString();
    		if (k + 1 < num_obstacles/*obstacles.size()*/)
    			s += "|";
	    }
    	s += "]";
    	return s;
    }


    // DEBUGGING
    public static void main(String args[]){
    	System.out.println("we start PlanMaker");
    	PlanMaker planmaker = new PlanMaker();

    	// set up visualize
    	Visualizer visu = new Visualizer(planmaker);
    	visu.go();
    }
        
	
}


// keep all the info for an obstacle
class Obstacle {

    boolean _isWall = false;		// is it the wall obstacle?
    int _num_vertices = 0;			// number of vertices
    LinkedList<Point2D> points = new LinkedList<Point2D>();		// points
    LinkedList<Point2D> points_ext = new LinkedList<Point2D>();
    Polygon poly = new Polygon();	// polygon, should be clockwise
    Polygon poly_ext;
    Polygon poly_ext2;
    boolean _lock = false;			// lock

    // constructors
    public Obstacle(int num_vertices){
    	_num_vertices = num_vertices;
    }

    public Obstacle(int num_vertices, boolean isWall){
    	_num_vertices = num_vertices;
    	_isWall = isWall;
    }


    // add point
    public void addVertice (double cX, double cY){
    	if (_lock)
    		try_unlock();
    	else
    		points.add(new Point2D.Double(cX,cY));
    }


    // lock, obstacle won't change
    // we compute some infos about it
    public void lock(){
    	if (_lock) return;
    	// create polygon
    	for (int k = 0; k < points.size(); k++)
    		poly.addPoint((int)points.get(k).getX(),(int)points.get(k).getY());
    	poly = PolyOps.make_clockwise(poly);
    	// expanded polygon
    	poly_ext = PolyOps.make_clockwise( PolyOps.expand(poly,Global.SAFETY_MARGIN) );
	double extra_safety = Global.SAFETY_MARGIN2;
	if (! isWall())
	    poly_ext2 = PolyOps.make_clockwise( PolyOps.expand(poly,Global.SAFETY_MARGIN+extra_safety) );
	else poly_ext2 = PolyOps.make_clockwise( PolyOps.expand(poly,extra_safety) );
    }

    // error message if we try to do a locked operation
    void try_unlock() {
    	System.out.println("illegal operation: obstacle locked");
    }

    
    // returns points of poly_ext2
    LinkedList<Point2D> possiblePathPoints(){
    	LinkedList<Point2D> res = new LinkedList<Point2D>();
    	// special case, just return point of poly
    	if (isWall()) {
    		for (int k = 0; k < getPoly().npoints; k++){
    			Point2D p= new Point2D.Double(getPoly().xpoints[k],getPoly().ypoints[k]);
    			res.add(p);
    		} 
    		return res;
    	}
    	// normal case, use poly_ext2
    	for (int k = 0; k < getPolyExt2().npoints; k++){
    		Point2D p= new Point2D.Double(getPolyExt2().xpoints[k],getPolyExt2().ypoints[k]);
    		res.add(p);
	    }
    	return res;
    }

    Point2D touch_where_wall(double x1, double y1, double x2, double y2) {
    	int delta = (int) Global.INTERSECT_PRECISION / 2;
    	// is segment vertical
    	if (x1 == x2) {
    		double y_small = y1;
    		double y_big = y2;
    		if (y1 > y2)
    			delta *= -1;
    		while (true){
    			if ( !poly.contains(x1,y_small) )
    				return new Point2D.Double(x1,y_small);
    			y_small += delta;
    			if (y1 < y2 && y_small > y2)
    				break;
    			else if (y_small < y2)
    				break;
    		}
    		if ( !poly.contains(x1,y_big) )
				return new Point2D.Double(x1,y_big);
    		return null;
    	}
    	// find function y = ax + b
    	double a, b;
    	if (x1 < x2)
    		a = (y2 - y1) * 1. / (x2 - x1);
		else
			a = (y1 - y2) * 1. / (x1 - x2);
    	b = y1 - a * x1;
    	double small_x = x1;
    	double big_x = x2;
    	if (x1 > x2)
    		delta *= -1;
    	while(true){
    		small_x += delta;
    		if ( !poly.contains(small_x,a * small_x+b) )
    			return new Point2D.Double(small_x,a*small_x+b);
    		small_x += delta;
    		if (x1<x2 && small_x > big_x)
    			break;
    		else if (small_x < big_x)
    			break;
	    }
    	if ( !poly.contains(big_x,a * big_x+b) )
			return new Point2D.Double(big_x,a*big_x+b);
    	return null;
    }
    
    // find where a segment touch the obstacle
    // (we look at poly)
    Point2D touch_where(double x1, double y1, double x2, double y2) {
    	if (isWall())
    		return touch_where_wall(x1,y2,x2,y2);
    	int delta = (int) Global.INTERSECT_PRECISION / 2;
    	// is segment vertical
    	if (x1 == x2) {
    		double y_small = y1;
    		double y_big = y2;
    		if (y1 > y2)
    			delta *= -1;
    		while (true){
    			if ( poly.contains(x1,y_small) )
    				return new Point2D.Double(x1,y_small);
    			y_small += delta;
    			if (y1 < y2 && y_small > y2)
    				break;
    			else if (y_small < y2)
    				break;
    		}
    		if ( poly.contains(x1,y_big) )
				return new Point2D.Double(x1,y_big);
    		return null;
    	}
    	// find function y = ax + b
    	double a, b;
    	if (x1 < x2)
    		a = (y2 - y1) * 1. / (x2 - x1);
		else
			a = (y1 - y2) * 1. / (x1 - x2);
    	b = y1 - a * x1;
    	double small_x = x1;
    	double big_x = x2;
    	if (x1 > x2)
    		delta *= -1;
    	while(true){
    		small_x += delta;
    		if ( poly.contains(small_x,a * small_x+b) )
    			return new Point2D.Double(small_x,a*small_x+b);
    		small_x += delta;
    		if (x1<x2 && small_x > big_x)
    			break;
    		else if (small_x < big_x)
    			break;
	    }
    	if ( poly.contains(big_x,a * big_x+b) )
			return new Point2D.Double(big_x,a*big_x+b);
    	return null;
    }
    

    // to handle special case of the wall
    // here we don't want to get out of poly
    // poly 2 is slightly expanded from poly in the wall case
    boolean exit_wall(double x1, double y1, double x2, double y2){
	//if (true) return false;
    	int delta = (int) Global.INTERSECT_PRECISION;

    	// check extrema
    	if (!poly_ext2.contains(x1,y1))
    		return true;
    	if (!poly_ext2.contains(x2,y2))
    		return true;
    	// is segment vertical
    	if (x1 == x2) {
    		double y_small = Math.min(y1,y2);
    		double y_big = Math.max(y1,y2);
    		while (y_small + delta < y_big){
    			y_small += delta;
    			if ( !poly_ext2.contains(x1,y_small) )
    				return true;
    		}
    		return false;
    	}
    	// find function y = ax + b
    	double a, b;
    	if (x1 < x2)
    		a = (y2 - y1) * 1. / (x2 - x1);
    	else 
    		a = (y1 - y2) * 1. / (x1 - x2);
    	b = y1 - a * x1;
    	double small_x = Math.min(x1,x2);
    	double big_x = Math.max(x1,x2);
    	while(small_x + delta < big_x){
    		small_x += delta;
    		if ( !poly_ext2.contains(small_x,a * small_x+b) )
    			return true;
	    }
    	return false;
    }

    // check if a line intersects with the extended polygon
    boolean intersect_ext(double x1, double y1, double x2, double y2){
    	// is wall? special case
    	if (isWall()) {
    		return exit_wall(x1,y1,x2,y2);
    	}

    	int delta = (int) Global.INTERSECT_PRECISION;
    	// check obvious cases
    	if (Math.max(x1,x2) < poly_ext.getBounds().getX())
    		return false;
    	if (Math.min(x1,x2) > poly_ext.getBounds().getX()+poly_ext.getBounds().getWidth())
    		return false;
    	if (Math.max(y1,y2) < poly_ext.getBounds().getY())
    		return false;
    	if (Math.min(y1,y2) > poly_ext.getBounds().getY()+poly_ext.getBounds().getHeight())
    		return false;
    	// check extrema
    	if (poly_ext.contains(x1,y1))
    		return true;
    	if (poly_ext.contains(x2,y2))
    		return true;
    	// is segment vertical
    	if (x1 == x2) {
    		double y_small = Math.min(y1,y2);
    		double y_big = Math.max(y1,y2);
    		while (y_small + delta < y_big){
    			y_small += delta;
    			if ( poly_ext.contains(x1,y_small) )
    				return true;
    		}
    		return false;
    	}
    	// find function y = ax + b
    	double a, b;
    	if (x1 < x2)
    		a = (y2 - y1) * 1. / (x2 - x1);
		else
			a = (y1 - y2) * 1. / (x1 - x2);
    	b = y1 - a * x1;
    	double small_x = Math.min(x1,x2);
    	double big_x = Math.max(x1,x2);
    	while(small_x + delta < big_x){
    		small_x += delta;
    		if ( poly_ext.contains(small_x,a * small_x+b) )
    			return true;
	    }
    	return false;
    }


    // getters & setters
    boolean isWall() { return _isWall; }
    
    void setWall(boolean isWall) { 
    	if (!_lock) _isWall = isWall;
    	else try_unlock();
    }
    
    int numVertices() { return _num_vertices; }
    
    Polygon getPoly() {
    	if (!_lock) return poly;
    	else { try_unlock(); return null; }
    }
    
    Polygon getPolyExt() {
    	if (!_lock) return poly_ext;
    	else { try_unlock(); return null; }
    }
    
    Polygon getPolyExt2() {
    	if (!_lock) return poly_ext2;
    	else { try_unlock(); return null; }
    }
    // number of points of poly
    int size() {
	if (!_lock) return poly.npoints;
	else { try_unlock(); return -1; }
    }


    // utility functions
    public String toString() {
    	String s = "Obstacle";
    	if (isWall())
    		s+= "(Wall)";
    	s += ",s="+((Integer)numVertices()).toString() + "[";
    	for (int k = 0; k < numVertices(); k++){
    		s += ((Double) points.get(k).getX() ).toString();
    		s += "," + ((Double) points.get(k).getY() ).toString();
    		if (k + 1 < numVertices())
    			s += ";";
	    }
    	s += "]";
    	return s;
    }
}




// class ot visualize stuff
class Visualizer extends JPanel implements ActionListener {

    private PlanMaker planmaker;			// planmaker
    boolean show_extborder = false;	// what to show
    boolean show_points = false;
    boolean show_allpaths = false;
    boolean show_finalpath = false;
    private boolean obstacles_are_set;
    private boolean start_goal_are_set;
    private boolean pathexecuted;
    private LinkedList<Point2D> path;
    private LinkedList<Point2D> executedpath;

    // files
    private String file_plan = "";
    private String file_start_goal = "";
    
    public Visualizer(PlanMaker pm){
    	planmaker = pm;
    	obstacles_are_set = false;
    	start_goal_are_set = false;
    	pathexecuted = false;
    }

    // launches everything
    public void go() {
    	JFrame frame = new JFrame("path planning");
    	frame.setDefaultCloseOperation(JFrame.DISPOSE_ON_CLOSE);
        this.setSize(new Dimension(Global.VISU_WIDTH,Global.VISU_HEIGHT));
        frame.setSize(this.getWidth() + 20 , this.getHeight() + 120);
        frame.setLayout(new BorderLayout());
        // add stuff
        frame.add(createVisualizerMenu(),BorderLayout.NORTH);
        frame.add(this,BorderLayout.CENTER);
        frame.add(createButtonBar(),BorderLayout.SOUTH);
        // make visible
        frame.setLocation(400,20);
        frame.setVisible(true);
    }

    // menu for visualizer
    private JMenuBar createVisualizerMenu()
    {
    	JMenuBar menubar = new JMenuBar();
    	// file menu
    	JMenu filemenu = new JMenu("File");
    	JMenuItem filemenu1 = new JMenuItem("Exit");
    	filemenu1.addActionListener(this);
    	filemenu.add( filemenu1 );
    	menubar.add( filemenu );
    	// load menu
    	JMenu loadmenu = new JMenu("Load");
    	JMenuItem loadmenu1 = new JMenuItem("Load obstacle file");
    	loadmenu1.addActionListener(this);
    	loadmenu.add(loadmenu1);
    	JMenuItem loadmenu2 = new JMenuItem("Load start goal file");
    	loadmenu2.addActionListener(this);
    	loadmenu.add(loadmenu2);
    	menubar.add( loadmenu );
    	// run menu
    	JMenu runmenu = new JMenu("run");
    	JMenuItem runmenu1 = new JMenuItem("go in testmode");
    	JMenuItem runmenu2 = new JMenuItem("just go");
    	runmenu1.addActionListener(this);
    	runmenu2.addActionListener(this);
    	runmenu.add( runmenu1 );
    	runmenu.add(runmenu2);
    	menubar.add( runmenu );
    	// done
    	return menubar;
    }
    
    // set visualizer size, scaling, ... for proper display
    private void setup_gui() {
    	// compute scaling and transposition
    	int adj = 10; // adjustment
    	double minX = planmaker.get_minX_obstacles();
    	double maxX = planmaker.get_maxX_obstacles();
    	double minY = planmaker.get_minY_obstacles();
    	double maxY = planmaker.get_maxY_obstacles();
    	double scaleX = (Global.VISU_WIDTH - adj) / (maxX - minX);
    	double scaleY = (Global.VISU_HEIGHT - adj) / (maxY - minY);
    	double scale = Math.max(scaleX,scaleY);
    	//System.out.println("Scaling = " + scale);
    	Global.SCALING = scale;
    	Global.X_TRANS = - minX * scale + adj ;
    	Global.Y_TRANS = - minY * scale + adj;
    }
    
    
    // main function, paint the plan, paths, borders, ...
    public void paint(Graphics g) {
    	if (file_plan == "")
    		return;
    	Graphics2D g2 = (Graphics2D)g;
    	int w = this.getWidth();
        int h = this.getHeight();
        // background
        g2.setColor(Color.lightGray);
        g2.fillRect(0, 0, w, h);
        // scaling and translations
        g2.translate(Global.X_TRANS,Global.Y_TRANS);
        g2.scale(Global.SCALING,Global.SCALING);

        g2.setColor(Color.black);
        if (obstacles_are_set)
        	for (int k = 0; k < planmaker.getNumObstacles(); k++){
        		Obstacle obst = planmaker.getObstacles().get(k);
        		if (! obst.isWall()) {
        			g2.setColor(Color.WHITE);
        			g2.fill(obst.getPoly());
        			g2.setColor(Color.BLACK);
        		}
        		g2.draw(obst.getPoly());
        	}

        // expansion
        if (show_extborder){
        	g2.setColor(Color.ORANGE);
        	for (int k = 0; k < planmaker.getNumObstacles(); k++){
        		Obstacle obst = planmaker.getObstacles().get(k);
        		if (obst.isWall())
        			continue;
        		g2.draw(obst.getPolyExt());
			// DEBUGGING
			//g2.setColor(Color.RED);
			//g2.draw(obst.getPolyExt2());
			//g2.setColor(Color.ORANGE);
		    }
	    }
	// possible points
	if (show_points) {
	        g2.setColor(Color.GREEN);
        	for (int i = 2; i < planmaker.getInterestingPoints().size(); i++){
        		Point2D p = planmaker.getInterestingPoints().get(i);
        		paint_point(g2,p);
        	}
        	// start and goal
        	g2.setColor(Color.RED);
        	if (start_goal_are_set){
        		paint_megapoint(g2,planmaker.getStart());
        		paint_megapoint(g2,planmaker.getGoal());
        	}
	}
	
        // possible paths
        if (show_allpaths){
        	g2.setColor(Color.GREEN);
        	Point2D p1, p2;
        	for (int k1 = 0; k1 < planmaker.getInterestingPoints().size() - 1; k1++)
        		for (int k2 = k1+1; k2 < planmaker.getInterestingPoints().size(); k2++){
        			if (planmaker.getdisttable()[k1][k2] > 0){
        				p1 = planmaker.getInterestingPoints().get(k1);
        				p2 = planmaker.getInterestingPoints().get(k2);
        				Line2D l = new Line2D.Double(p1,p2);
        				g2.draw(l);
        			}
        		}
        }
	// final path
        if (show_finalpath){
        	Point2D p1, p2;
        	g2.setColor(Color.RED);
        	for (int k1 = 0; k1 < path.size() - 1; k1++){
        		p1 = path.get(k1);
        		p2 = path.get(k1+1);
        		paint_point(g2,p1);
        		paint_point(g2,p2);
        		Line2D l = new Line2D.Double(p1,p2);
        		g2.draw(l);
        	}
        	paint_megapoint(g2,planmaker.getStart());
        	paint_megapoint(g2,planmaker.getGoal());
	    }
        if (pathexecuted){
        	Point2D p1, p2;
        	g2.setColor(Color.CYAN);
        	for (int k1 = 0; k1 < executedpath.size() - 1; k1++){
        		p1 = executedpath.get(k1);
        		p2 = executedpath.get(k1+1);
        		paint_point(g2,p1);
        		paint_point(g2,p2);
        		Line2D l = new Line2D.Double(p1,p2);
        		g2.draw(l);
        	}        	
        }
    }

    
    // paint a simple point, color assumed to be set
    private void paint_point(Graphics2D g2, Point2D p) {
    	paint_point(g2,p.getX(),p.getY());
    }
    private void paint_point(Graphics2D g2, double x, double y){
    	int radius = Global.POINT_RADIUS;
    	Ellipse2D e= new Ellipse2D.Double(x-radius/2,y-radius/2,radius,radius);
    	g2.fill(e);
    }
    // paint a complex point, like start and goal
    private void paint_megapoint(Graphics2D g2, Point2D p) {
    	paint_megapoint(g2,p.getX(),p.getY());
    }
    private void paint_megapoint(Graphics2D g2, double x, double y){
    	int radius1 = Global.POINT_RADIUS;
    	int radius2 = (int) (radius1 * 1.2);
    	int radius3 = (int) (radius1 * 1.4);
    	Color c = g2.getColor();
    	Ellipse2D e1= new Ellipse2D.Double(x-radius1/2,y-radius1/2,radius1,radius1);
    	Ellipse2D e2= new Ellipse2D.Double(x-radius2/2,y-radius2/2,radius2,radius2);
    	Ellipse2D e3= new Ellipse2D.Double(x-radius3/2,y-radius3/2,radius3,radius3);
    	g2.fill(e3);
    	g2.setColor(Color.WHITE);
    	g2.fill(e2);
    	g2.setColor(c);
    	g2.fill(e1);
    }
    
    
    // create control bar
    protected JPanel createButtonBar() {
    	JPanel panel = new JPanel();
    	// add buttons
    	panel.add( createNoneButton() );
    	panel.add( createExtButton() );
    	panel.add( createPointsButton() );
    	panel.add( createAllPathsButton() );
    	panel.add( createPathButton() );
    	panel.add( createAllButton() );
    	//panel.add( createGoButton());
    	// done
    	return panel;
    }
    
    // create buttons to control visualization
    protected JButton createNoneButton() {
    	JButton button =  new JButton("None");
    	button.addActionListener(this);
    	return button;
    }
    
    protected JButton createExtButton() {
    	JButton button = new JButton("Borders");
    	button.addActionListener(this);
    	return button;
    }
    
    protected JButton createPointsButton() {
    	JButton button = new JButton("Points");
    	button.addActionListener(this);
    	return button;
    }

    protected JButton createAllPathsButton() {
    	JButton button = new JButton("All Paths");
    	button.addActionListener(this);
    	return button;
    }
    
    protected JButton createPathButton() {
    	JButton button = new JButton("Final Path");
    	button.addActionListener(this);
    	return button;
    }
    
    protected JButton createAllButton() {
    	JButton button = new JButton("All");
    	button.addActionListener(this);
		return button;
    }
    
    protected JButton createGoButton() {
    	JButton button = new JButton("Go");
    	button.addActionListener(this);
		return button;
    }

    // commands from buttons

    public void actionPerformed(ActionEvent e){
    	if ("None".equals(e.getActionCommand())){
    		show_extborder = false;
    		show_points = false;
    		show_allpaths = false;
    		show_finalpath = false;
    		pathexecuted = false;
	    }
    	else if ("Borders".equals(e.getActionCommand())){
    		if (file_plan == "")
    			return;
    		if (show_extborder)
    			show_extborder = false;
    		else
    			show_extborder = true;
	    }
    	else if (e.getActionCommand() == "Points"){
    		if (file_start_goal == "")
    			return;
    		if (show_points)
    			show_points = false;
    		else
    			show_points = true;
	    }
    	else if (e.getActionCommand() == "All Paths"){
    		if (file_start_goal == "")
    			return;
	        if (show_allpaths)
    			show_allpaths = false; 
    		else {
    			show_allpaths = true;
			show_points = true;
		}
	}
    	else if (e.getActionCommand() == "Final Path"){
    		if (file_start_goal == "")
    			return;
    		if (show_finalpath)
    			show_finalpath = false;
    		else
    			show_finalpath = true;
	    }
    	else if (e.getActionCommand() == "All"){
    		if (file_start_goal == "")
    			return;
    		show_extborder = true;
    		show_points = true;
    		show_allpaths = true;
    		show_finalpath = true;
	    }
    	else if (e.getActionCommand() == "just go"){
    		if (obstacles_are_set && start_goal_are_set){
    			executedpath = planmaker.executePath(path, false);
    			pathexecuted = true;
    		}
    		else{
    			if (!obstacles_are_set)
    				System.out.println("Set the obstacles first!");
    			if (!start_goal_are_set)
    				System.out.println("Set the start and goal first!");
    		}
    	}
    	else if (e.getActionCommand() == "go in testmode"){
    		if (obstacles_are_set && start_goal_are_set){
    			int count = 0;
    			while (executedpath == null && count < 2){
    			  executedpath = planmaker.executePath(path, true);
    			  /* Needs to be finished
    			  possibleP = new LinkedList<Point2D>();
    			  planmaker.read_obstacle_file( file_plan );
    			  planmaker.read_startgoal_file( file_start_goal );
    			  */
    			  count++;
    			}
    			pathexecuted = true;
    		}
    		else{
    			if (!obstacles_are_set)
    				System.out.println("Set the obstacles first!");
    			if (!start_goal_are_set)
    				System.out.println("Set the start and goal first!");
    		}
    	}
    	else if ("Exit".equals(e.getActionCommand())) {
    		System.exit(0);
    	}
    	else if ("Load obstacle file".equals(e.getActionCommand())) {
	    	JFileChooser chooser = new JFileChooser(".");
	        int returnVal = chooser.showOpenDialog(this);
	        if(returnVal == JFileChooser.APPROVE_OPTION) {
	           file_plan = chooser.getSelectedFile().getName();
	           planmaker.read_obstacle_file( file_plan );
	           if (planmaker.getObstacles()!=null){
	        	   setup_gui();
	        	   show_extborder = true;
	        	   obstacles_are_set = true;
	        	   if (start_goal_are_set)
	        		   path = planmaker.dijkstraPath();
	           }
	           else{
	        	   obstacles_are_set = false;
	           }
        	   pathexecuted = false;
        	   executedpath=null;
	        }
    	}
    	else if ("Load start goal file".equals(e.getActionCommand())) {
    		if (obstacles_are_set){
    			JFileChooser chooser = new JFileChooser(".");
    			int returnVal = chooser.showOpenDialog(this);
    			if(returnVal == JFileChooser.APPROVE_OPTION) {
    				file_start_goal = chooser.getSelectedFile().getName();
    				planmaker.read_startgoal_file( file_start_goal );
    				if (planmaker.getStart()!=null && planmaker.getGoal()!=null){
    					// visualization
  						path = planmaker.dijkstraPath();
   						show_extborder = true;
    					show_points = true;
    					show_allpaths = true;
    					show_finalpath = true;
    					start_goal_are_set = true;
    				}
    				else{
    					start_goal_are_set = false;
    				}
					pathexecuted = false;
					executedpath = null;
    			}
    		}
    		else{
    			System.out.println("Set obstacle file first!");
    		}
    	}
    	else{
    		System.out.println("weird action command: " + e.getActionCommand());
	    }
    	repaint();
    }

}
